/*
 * TODO: Description
 *
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Testing.h"
#include "ModelSimulator.h"

#include <stdio.h>
#include <tchar.h>
#include <iostream>
using namespace std;
using namespace OpenFms;


bool fileExists( TCHAR *filename )
{
	cout << "Find file: " << filename << " ... ";
	FILE *file = _tfopen( filename, _T("rb") );
	fclose(file);

	if( file == NULL ) {
		cout << _T("FAIL") << endl;
		return false;
	}
	cout << _T("OK")  << endl;
	return true;
}

bool loadMockModel( ModelSimulator &ms, const TCHAR *filename )
{
	cout << "Loading model ... ";
	if( ms.load( filename, filename ) == false ) {
		cout << _T("FAIL") << endl;
		return false;
	}
	cout << _T("OK")  << endl;
	return true;
}

void setupZeroEnvironment( ModelSimulator_environment_t &environment )
{
	environment.size = sizeof environment;
	environment.linearVelocityX = 0.f;
	environment.linearVelocityY = 0.f;
	environment.linearVelocityZ = 0.f;
	for( int i=0; i<16; ++i ) environment.channel[i] = 0.f;
}

int _tmain(int argc, TCHAR *argv[])
{
	if ( argc != 2) {
		cout << "SYNTAX: ... <MockModel library filename>" << endl;
		return 1;
	}
	TCHAR *filename = argv[1];
	ModelSimulator ms;

	// If we don't find the file, or can't load the model
	// then don't even bother continue
	if( fileExists(filename) == false ) return 1;
	if( loadMockModel( ms, filename ) == false ) return 1;

	ModelSimulator_environment_t environment = { sizeof(ModelSimulator_environment_t) };
	ModelSimulator_inertias_t inertias = { sizeof(ModelSimulator_inertias_t) };
	ModelSimulator_forces_t forces = { sizeof(ModelSimulator_forces_t) };

	PRE_TESTING();

	// from MockModel doc:
	// force_x is set to the value 0.123f;
	// force_y is set to the value specified by the channel 1 
	// force_z is set to sin ( 3.14 * channel 2 ) (i.e. [-1,1] => [ sin(-3.14), sin(3.14) ]
	// torque_x is set to the value wind_x in the environment structure
	// torque_y is set to the value wind_y in the environment structure
	// torque_z is set to the value wind_z in the environment structure

	cout << "TEST - All zeros" << endl;
	setupZeroEnvironment(environment);
	UNITTEST_BOOL_EQUALS( true, ms.step( environment, forces ) );
	UNITTEST_FLOAT_EQUALS( 0.123f, forces.force_x )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.force_y )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.force_z )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.torque_x )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.torque_y )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.torque_z )

	cout << "TEST - Winds" << endl;
	setupZeroEnvironment(environment);
	environment.linearVelocityX = 1.11f;
	environment.linearVelocityY = 22.2f;
	environment.linearVelocityZ = 12.34f;
	UNITTEST_BOOL_EQUALS( true, ms.step( environment, forces ) );
	UNITTEST_FLOAT_EQUALS( 0.123f, forces.force_x )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.force_y )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.force_z )
	UNITTEST_FLOAT_EQUALS( 1.11f, forces.torque_x )
	UNITTEST_FLOAT_EQUALS( 22.2f, forces.torque_y )
	UNITTEST_FLOAT_EQUALS( 12.34f, forces.torque_z )

	cout << "TEST - Channels" << endl;
	setupZeroEnvironment(environment);
	environment.channel[0] = 11.22f;
	environment.channel[1] = .25f;
	UNITTEST_BOOL_EQUALS( true, ms.step( environment, forces ) );
	UNITTEST_FLOAT_EQUALS( 0.123f, forces.force_x )
	UNITTEST_FLOAT_EQUALS( 11.22f, forces.force_y )
	UNITTEST_FLOAT_EQUALS( sqrt(.5f), forces.force_z )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.torque_x )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.torque_y )
	UNITTEST_FLOAT_EQUALS( 0.f, forces.torque_z )

	cout << "TEST - Winds and channels" << endl;
	setupZeroEnvironment(environment);
	environment.linearVelocityX = 1.11f;
	environment.linearVelocityY = 22.2f;
	environment.linearVelocityZ = 12.34f;
	environment.channel[0] = 11.22f;
	environment.channel[1] = 1.f/6;
	UNITTEST_BOOL_EQUALS( true, ms.step( environment, forces ) );
	UNITTEST_FLOAT_EQUALS( 0.123f, forces.force_x )
	UNITTEST_FLOAT_EQUALS( 11.22f, forces.force_y )
	UNITTEST_FLOAT_EQUALS( .5f, forces.force_z )
	UNITTEST_FLOAT_EQUALS( 1.11f, forces.torque_x )
	UNITTEST_FLOAT_EQUALS( 22.2f, forces.torque_y )
	UNITTEST_FLOAT_EQUALS( 12.34f, forces.torque_z )

	// todo: test angular velocity
	POST_TESTING();
}





